#include <stdio.h>
#include "os/queue.h"
#include "os/os.h"
#include "bsp/cpu_driver.h"
#include "utils/utils.h"
#include "utils/circle_buffer.h"
#include "communication/message.h"

#define CAN_RX_MESSAGE_RX_ID 1
#define CAN_SEND_QUEUE_SIZE 32
#define RX_ID_OFFSET 16
#define CAN_SEND_OK 0
#define CAN_SEND_ERROR -1
#define CAN_SEND_WAIT_TIMEOUT -2


#define TX_NUM 64
#define RX_NUM 64
static co_queue_t g_tx_queue;
static co_queue_t g_rx_queue;
static uint8_t _g_tx_buffer[sizeof(can_trasnmit_message_struct) * TX_NUM + 1];
static uint8_t _g_rx_buffer[sizeof(can_receive_message_struct) * RX_NUM + 1];
static can_frame_handler _handler = NULL;
static int can_send_data(can_trasnmit_message_struct *P_message);
static uint8_t can_get_mailbox(uint32_t can_periph);
/* this function can be overide by app, which need recv the can frame */
__weak void handle_can_frame(u32 id, uint8_t *data, int len){

}

void can_rx_poll(void){
	can_receive_message_struct message;
	if (queue_get(g_rx_queue, &message) && (_handler != NULL)) {
		_handler(message.rx_efid, message.rx_data, message.rx_dlen);
	}
}

void can_tx_poll(void){
	can_trasnmit_message_struct can_tr_m;
	if (CAN_ERR(CAN0) & CAN_ERR_BOERR){
		can_drv_reset();
	}
	while (can_get_mailbox(CAN0) != CAN_NOMAILBOX) {
		if (!queue_get(g_tx_queue, &can_tr_m)) {
			break;
		}
		can_message_transmit(CAN0,&can_tr_m);
	}
}

static u32 _can_poll_task(void *args) {
	can_rx_poll();
	can_tx_poll();
	return 0;
}

static __inline__ void can_fifo_recv(int fifo){
	can_receive_message_struct Rxmessage;

	can_message_receive(CAN0, fifo, &Rxmessage);

	queue_put(g_rx_queue, &Rxmessage);

}

void CAN_RX0_IRQHandler(void)
{
	can_fifo_recv(CAN_FIFO0);
}

void CAN0_RX1_IRQHandler(void)
{
	can_fifo_recv(CAN_FIFO1);
}

static void can_io_init(void){
    /* enable can clock */
    rcu_periph_clock_enable(RCU_CAN0);

    gpio2_af_init(CAN_TX_PIO, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, CAN_REMAP);
	gpio2_af_init(CAN_RX_PIO, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, CAN_REMAP);
}


static void can_config(void)
{
    can_parameter_struct can_parameter;
     
    can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
    /* initialize CAN register */
    can_deinit(CAN0);
    //can_deinit(CAN1);
    
    /* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = ENABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.rec_fifo_overwrite = DISABLE;
	can_parameter.trans_fifo_order = ENABLE;
	can_parameter.no_auto_retrans = ENABLE;
#if CONFIG_CAN_SPEED==250 //250k bps
	if (SystemCoreClock == 120000000) {
		can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
		can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
		can_parameter.time_segment_2 = CAN_BT_BS2_4TQ;
	} else {
		can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
		can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
		can_parameter.time_segment_2 = CAN_BT_BS2_3TQ;
	}
#endif
	can_parameter.working_mode = CAN_NORMAL_MODE;
	can_parameter.prescaler = 24;
    /* initialize CAN */
    can_init(CAN0, &can_parameter);
    
    /* recv my can ID, use fifo0 */
    can_filter_mask_mode_init(MY_MESSAGE_ADDR, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO0, 0);
 
  	nvic_irq_enable(CAN_IRQ0,CAN_IRQ_PRIORITY,0);	
    /* enable can receive FIFO0 not empty interrupt */
    can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
}


static int can_send_data(can_trasnmit_message_struct *P_message){
	can_tx_poll();
	int len = sizeof(can_trasnmit_message_struct);
	if (queue_put(g_tx_queue, P_message)){
		return CAN_SEND_OK;
	}
	return CAN_SEND_ERROR;
}


static uint8_t can_get_mailbox(uint32_t can_periph)
{
    uint8_t mailbox_number = CAN_MAILBOX0;
    
    /* select one empty mailbox */
    if(CAN_TSTAT_TME0 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME0)){
        mailbox_number = CAN_MAILBOX0;
    }else if(CAN_TSTAT_TME1 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME1)){
        mailbox_number = CAN_MAILBOX1;
    }else if(CAN_TSTAT_TME2 == (CAN_TSTAT(can_periph)&CAN_TSTAT_TME2)){
        mailbox_number = CAN_MAILBOX2;
    }else{
        mailbox_number = CAN_NOMAILBOX;
    }
    /* return no mailbox empty */
    if(CAN_NOMAILBOX == mailbox_number){
        return CAN_NOMAILBOX;
    }
	return mailbox_number;
}


int can_send_message(uint32_t can_id, const void*buff, int len){
	can_trasnmit_message_struct trasnmit_msg;
	head_t can_frame_id;
	u32 total_frames = ((len + 7) >> 3);
	int send_len = len;
	u32 frame_id = 1;
	can_frame_id.id = can_id;
	can_frame_id.total = total_frames;

	while(send_len > 0){
		can_frame_id.idx = frame_id;

		trasnmit_msg.tx_sfid	= 0;
		trasnmit_msg.tx_efid	= can_frame_id.id;				
		trasnmit_msg.tx_ft	= CAN_FT_DATA;
		trasnmit_msg.tx_ff	= CAN_FF_EXTENDED;
		trasnmit_msg.tx_dlen = _min(8,send_len);
		memcpy((char *)trasnmit_msg.tx_data, (char *)buff + (len - send_len), trasnmit_msg.tx_dlen);
		send_len -= trasnmit_msg.tx_dlen;
		frame_id ++;
		if (can_send_data(&trasnmit_msg) != CAN_SEND_OK){
			return CAN_SEND_ERROR;
		}
	}

	return CAN_SEND_OK;

}

int can_send_ext_message(uint32_t can_id, const void*buff, int len){
	can_trasnmit_message_struct trasnmit_msg;
	
	trasnmit_msg.tx_sfid	= 0;
	trasnmit_msg.tx_efid	= can_id;				
	trasnmit_msg.tx_ft	= CAN_FT_DATA;
	trasnmit_msg.tx_ff	= CAN_FF_EXTENDED;
	trasnmit_msg.tx_dlen = _min(8,len);
	memcpy((char *)trasnmit_msg.tx_data, (char *)buff, trasnmit_msg.tx_dlen);

	if (can_send_data(&trasnmit_msg) != CAN_SEND_OK){
		return CAN_SEND_ERROR;
	}
	return CAN_SEND_OK;
}


void can_drv_reset(void){
	can_io_init();
	can_config();
}

void can_drv_deinit(void){
	can_deinit(CAN0);
	rcu_periph_clock_disable(RCU_CAN0);
}

void can_drv_init(can_frame_handler handler){
	_handler = handler;
	g_tx_queue = queue_create_with_buffer(_g_tx_buffer, sizeof(_g_tx_buffer), sizeof(can_trasnmit_message_struct));
	g_rx_queue = queue_create_with_buffer(_g_rx_buffer, sizeof(_g_rx_buffer), sizeof(can_receive_message_struct));
	os_task_create(_can_poll_task, NULL);
	can_io_init();
	can_config();
}

